Search Algorithms¶
Part 7 - A Star (contd)¶
In last section, I mentioned our algorithm still fails in case of 'A' to 'U'. Below is our current algorithm finding its route to 'U'.
Found path is ['A','S','F','B','U'] which costs 138+41+112+33 = 324
Cheaper path: is ['A','S','RV','P','B','U'] which costs 138+49+29+67+33 = 316
Note after reaching 'U', the traceback path is currently possible only via 'F' instead of 'P'. Think why
Let us also once run the algorithm and confirm to you, it returning costlier path 324
from queue import PriorityQueue
from math import floor
import geopy
from docHelpers_ipython import romania_location_map
cameFrom = { }
# NEEDED FUNCTION TO CALCULATE DISTANCE FROM CURRENT NODE BACK TO START
def totalDistance(current_node):
cost = 0
while current_node is not None:
parent = cameFrom.get(current_node, None)
if parent is not None:
cost += distance(current_node, parent)
current_node = parent
return cost
def distance(start_node, end_node):
(x0,y0) = M[start_node]['pos']
(x1,y1) = M[end_node]['pos']
return floor(geopy.distance.geodesic((y0,x0), (y1,x1)).miles)
def reconstructPath(current_node):
Path = [current_node]
while current_node is not None:
current_node = cameFrom[current_node]
Path.append(current_node)
return reversed(Path[:-1])
def AStar(start, goal):
# INITIALIZATION
openSet = PriorityQueue()
openSet.put((0,start))
closedSet = set(start)
cameFrom[start] = None
# MAIN LOOP
while not openSet.empty():
current_cost, current_node = openSet.get()
if current_node is goal:
print('Success. Route from {} to {} found. Cost: {} Path: {}'.format(start,goal,current_cost,list(reconstructPath(current_node))))
break
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors:
if each_neighbor not in closedSet:
#gScore = 'cost till current_node' + 'cost between current node and its neighbor'
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
#print(openSet.queue) #just to verify
return 'No Goal found!'
M = romania_location_map
start = 'A'
goal = 'U'
result = AStar(start, goal)
So what is happening? Note the animation again carefully.
'F'processed before'P'- When
'F'was current node,'B'being one of neighbors, would have been added to'closedSet'. - Also this happened:
cameFrom['B'] = 'F' - When later
'P'was processed,'B'was simply ignored because it was already visited.
If some how here, we made an update cameFrom['B']='P', we would have gotten the cheaper path.
I get it. It is difficult to still connect the dots. So we will go again manually step by step, tracing also closedSet and cameFrom this time. Its 10 iterations, so please bear with me, its worth the patience.
Start
Let us initialize as usual..
openSet = { (0,'A') }
closedSet = ( 'A' )
cameFrom[ 'A' ] = NoneCoding..
from queue import PriorityQueue
from docHelpers_ipython import romania_location_map
cameFrom = { }
openSet = PriorityQueue()
M = romania_location_map
start = 'A'
goal = 'U'
openSet.put((0,start))
closedSet = set(start)
cameFrom[start] = None
ITERATION 1
Is openSet empty? No. So Go on.
Take least cost path from openSet
{(0, 'A')} : (0, 'A')
Current node: 'A'. Kids: 'S','T','Z'
Is 'A' the goal? No. So Go on.
closedSet : ( 'A' )
'A' to 'S':
past cost from 'A' to 'S' : 138
heuristic cost from 'S' to 'U' : 143
total : 281
closedSet : ( 'A', 'S' )
'A' to 'T':
past cost from 'A' to 'T' : 30
heuristic cost from 'T' to 'U' : 274
total : 304
closedSet : ( 'A', 'S', 'T' )
'A' to 'Z':
past cost from 'A' to 'Z' : 31
heuristic cost from 'Z' to 'O' : 280
total : 311
closedSet : ( 'A', 'S', 'T', 'Z' )
Result: openSet = { (281,'S'), (304,'T'), (311,'Z') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
Visualizing..(do not bother about order of added nodes)
from docHelpers_ipython import Doc
from IPython.core.display import HTML
doc = Doc(M)
resultHTML = doc.computeGraphs('A',['T','S','Z'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
ITERATION 2
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (281, 'S'), (304, 'T'), (311, 'Z') } : (281,'S')
Current node: 'S'. Kids: 'F','RV','O'
Is 'S' the goal? No. So Go on.
closedSet : ( 'A', 'S', 'T', 'Z' )
'S' to 'F':
past cost from 'A' to 'F' : 138+41
heuristic cost from 'F' to 'U' : 112
total : 291
closedSet : ( 'A', 'S', 'T', 'Z', 'F' )
'S' to 'RV':
past cost from 'A' to 'RV' : 138+49
heuristic cost from 'RV' to 'U' : 114
total : 301
closedSet : ( 'A', 'S', 'T', 'Z', 'F', 'RV' )
'S' to 'O':
past cost from 'A' to 'O' : 138+136 (not 31+34. why?)
heuristic cost from 'O' to 'U' : 278
total : 552
closedSet : ( 'A', 'S', 'T', 'Z', 'F', 'RV', 'O' )
Result: openSet = { (304, 'T'), (311, 'Z'), (291, 'F'), (301, 'RV'), (552, 'O') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('S',['F','RV','O'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
ITERATION 3
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (291, 'F'), (301, 'RV'), (304, 'T'), (311, 'Z'), (552, 'O') } : (291, 'F')
Current node: 'F'. Kids: 'B'
Is 'S' the goal? No. So Go on.
closedSet : ( 'O', 'RV', 'S', 'A', 'Z', 'T', 'F' )
'F' to 'B':
past cost from 'A' to 'B' : 138+41+112
heuristic cost from 'B' to 'U' : 33
total : 324
closedSet : ( 'O', 'RV', 'S', 'A', 'Z', 'T', 'F', 'B' )
Result: openSet = { (301, 'RV'), (304, 'T'), (311, 'Z'), (552, 'O'), (324, 'B') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('F',['B'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
ITERATION 4
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (301, 'RV'), (311, 'Z'), (304, 'T'), (552, 'O'), (324, 'B') } : (301, 'RV')
Current node: 'RV'. Kids: 'C','P'
Is 'RV' the goal? No. So Go on.
closedSet : ( 'RV', 'Z', 'T', 'A', 'S', 'F', 'B', 'O' )
'RV' to 'C':
past cost from 'A' to 'C' : 138+49+60
heuristic cost from 'C' to 'U' : 143
total : 390
closedSet : ( 'RV', 'Z', 'T', 'A', 'S', 'F', 'B', 'O', 'C' )
'RV' to 'P':
past cost from 'A' to 'P' : 138+49+29
heuristic cost from 'P' to 'U' : 87
total : 303
closedSet : ( 'RV', 'Z', 'T', 'A', 'S', 'F', 'B', 'O', 'C', 'P' )
Result: openSet = { (311, 'Z'), (304, 'T'), (552, 'O'), (324, 'B'), (390, 'C'), (303,'P') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('RV',['C','P'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
ITERATION 5
Is q empty? No. So Go on.
Take least cost path from q
{ (303, 'P'), (311, 'Z'), (304, 'T'), (552, 'O'), (390, 'C'), (324, 'B') } : (303, 'P')
Current node: 'P'. Kids: 'B' which is already visited, so do nothing
closedSet : ( 'A', 'B', 'F', 'O', 'P', 'S', 'T', 'Z', 'RV', 'C' )
Result: q = { (311, 'Z'), (304, 'T'), (552, 'O'), (390, 'C'), (324, 'B') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('P',[], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
Breakpoint!
Wait, wait, wait!! Note at this juncture, cameFrom['B']='F' and we are proceeding doing nothing about it since 'B' was arleady visited.
What if, some how if we could justify, updating to cameFrom['B']='P', the current node??
We could use cost itself for that. Let us check, so far which is better w.r.t cost?
gScore1('B') via 'F' = 138+41+112 = 291
gScore2('B') via 'P' = 138+49+29+67 = 283
We will see, how to calculate later, but gScore2 is cheaper than gScore1, so 'B' deserves 'P' as parent, more than 'F'.
Hold on this thought. We will finish rest of iterations (just 5 more(?!)) and come back.
Proceed..
ITERATION 6
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (304, 'T'), (311, 'Z'), (324, 'B'), (552, 'O'), (390, 'C') } : (304, 'T')
Current node: 'T'. Kids: 'LU'
Is 'T' the goal? No. So Go on.
closedSet : ( 'P', 'O', 'Z', 'F', 'B', 'C', 'T', 'RV', 'S', 'A' )
'T' to 'LU':
past cost from 'A' to 'LU' : 30+33
heuristic cost from 'LU' to 'U' : 240
total : 303
closedSet : ( 'P', 'O', 'Z', 'F', 'B', 'C', 'T', 'RV', 'S', 'A', 'LU' )
Result: openSet = { (311, 'Z'), (324, 'B'), (552, 'O'), (390, 'C'), (303, 'LU') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('T',['LU'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
Ah, yeah its tiring. We were so close, and kinda back to pavillion. Hold on, Comrades!
ITERATION 7
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (303, 'LU'), (311, 'Z'), (324, 'B'), (552, 'O'), (390, 'C') } : (303, 'LU')
Current node: 'LU'. Kids: 'M'
Is 'LU' the goal? No. So Go on.
closedSet : ( 'P', 'O', 'LU', 'Z', 'F', 'B', 'C', 'T', 'RV', 'S', 'A' )
'LU' to 'M':
past cost from 'A' to 'M' : 30+33+58
heuristic cost from 'LU' to 'U' : 210
total : 331
closedSet : ( 'P', 'O', 'LU', 'Z', 'F', 'B', 'C', 'T', 'RV', 'S', 'A', 'M' )
Result: openSet = { (311, 'Z'), (324, 'B'), (552, 'O'), (390, 'C'), (331, 'M') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('LU',['M'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
ITERATION 8
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (311, 'Z'), (331, 'M'), (324, 'B'), (552, 'O'), (390, 'C') } : (311, 'Z')
Current node: 'Z'. Kids: 'O' which is already visited so no action
Is 'LU' the goal? No. So Go on.
closedSet : ( 'P', 'O', 'LU', 'M', 'Z', 'F', 'B', 'C', 'T', 'RV', 'S', 'A' )
Result: openSet = { (331, 'M'), (324, 'B'), (552, 'O'), (390, 'C'), }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('Z',[], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
ITERATION 9
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (324, 'B'), (331, 'M'), (390, 'C'), (552, 'O') } : (324, 'B')
Current node: 'B'. Kids: 'U'
Is 'B' the goal? No. So Go on.
closedSet : ( 'P', 'O', 'LU', 'M', 'Z', 'F', 'B', 'C', 'T', 'RV', 'S', 'A' )
'B' to 'U':
past cost from 'A' to 'U' : 138+41+112+33 NOTE! SINCE B PARENT IS F, WE CALCULATE VIA THAT PATH :(
heuristic cost from 'U' to 'U' : 0
total : 324
closedSet : ( 'P', 'O', 'LU', 'M', 'Z', 'F', 'B', 'C', 'T', 'RV', 'S', 'A', 'U' )
Result: openSet = { (331, 'M'), (390, 'C'), (552, 'O'), (324, 'U') }
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('B',['U'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
ITERATION 10
Is openSet empty? No. So Go on.
Take least cost path from openSet
{ (324, 'U')), (331, 'M'), (390, 'C'), (552, 'O'), (392, 'G') } : (324, 'B')
Current node: 'U'. Kids: 'V'
Is 'U' the goal? YES. Return.
current_cost: 324
current_node: 'U'
current_path: ( 'A', 'S', 'F', 'B', 'U')
if not openSet.empty(): # Note unike deque, we check differently using empty() if our bag is empty or not
current_cost, current_node = openSet.get() # Note, we 'get()' now and it returns a 'set', a tuple
if current_node is goal:
print('Yes. Success. Current node: ', current_node)
if current_node is not goal:
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors: # each neighbor is a 'set' having a cost and node itself
if each_neighbor not in closedSet:
#NOTE: fScore = gScore + hScore
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
print(list(openSet.queue))
print(list(closedSet))
print(cameFrom)
resultHTML = doc.computeGraphs('U',['V'], mappy=True, tree=True, queue=True, HTML=True)
HTML(resultHTML)
Finally!! Let us get back to iteration 5
In Iteration 5, when current_node was 'P' and neighbor was 'B' we simply did nothing. Instead, if we had replaced parent of 'B' from 'F' to 'P', our path would have changed later for better. Isnt it? But what could be justification to do that?
Recall in Iteration 5. cameFrom['B'] was already 'F'. We now consider 'P'. Which could be better parent? (Eh?!)
We will choose better parent which has least g('B') from paths via 'F' and via 'P'.
```cameFrom``` during Iteration 5 which we could use to calculate:
{'A': None, 'S': 'A', 'T': 'A', 'Z': 'A', 'F': 'S', 'RV': 'S', 'O': 'S', 'B': 'F', 'C': 'RV', 'P': 'RV'}
1) path to 'B' via 'F' : ('B' -> 'F' -> 'S' -> 'A') costs 112+41+138 = 291
2) path to 'B' via 'P' : ('B' -> 'P') + ('P' -> 'RV' -> 'S' -> 'A') costs (67) + (29+49+138) = 283
Note we calculate ('B' -> 'P') separetely because this is not yet in cameFrom (which is what we are going to decide about). So we cannot use 'totalDistance' directly which uses 'cameFrom ' list.
Re read and make sure you understand above explanation, as this is very vital for optimization we would do next
To differentiate 2 calculations above, we will call one has g('B') or actual g score. The other one as t_g('B') or tentative g score
Thus
1) t_g('B') or tentative_gScore = totalDistance('B') = 291
2) g('B') or gScore = totalDistance('P') + distance('P','B') = 283
if g('B') is minimal of above two, then update as cameFrom['B'] = 'P'
As we saw above, g('B') < t_g('B'), that is 283 < 291. so we should update cameFrom['B'] = 'P'
Let us try to insert this change in the code and see what happens.
Remember 'B' was child here, and 'P' and 'F' were potential parents ('P' was current node and 'F' was existing in cameFrom as current parent of 'B')
Do not worry, not another 10 manual iterations. Let us directly inject this change in entire modularized current A Star algorithm and find how it works. Below would be our relevant snippet. This time, not ignoring visited nodes, we say,..
if each_neighbor in closedSet:
tentative_gScore = totalDistance(each_neighbor) # neighbor already in closed set, so this shld work
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
if (gScore < tentative_gScore): # current_node is better parent for this neighbor
cameFrom[each_neighbor] = current_node
from queue import PriorityQueue
from docHelpers_ipython import romania_location_map
cameFrom = { }
# NEEDED FUNCTION TO CALCULATE DISTANCE FROM CURRENT NODE BACK TO START
def totalDistance(current_node):
cost = 0
while current_node is not None:
parent = cameFrom.get(current_node, None)
if parent is not None:
cost += distance(current_node, parent)
current_node = parent
return cost
def distance(start_node, end_node):
(x0,y0) = M[start_node]['pos']
(x1,y1) = M[end_node]['pos']
return floor(geopy.distance.geodesic((y0,x0), (y1,x1)).miles)
def reconstructPath(current_node):
Path = [current_node]
while current_node is not None:
current_node = cameFrom[current_node]
Path.append(current_node)
return reversed(Path[:-1])
def AStar(start, goal):
# INITIALIZATION
openSet = PriorityQueue()
openSet.put((0,start))
closedSet = set(start)
cameFrom[start] = None
# MAIN LOOP
while not openSet.empty():
current_cost, current_node = openSet.get()
if current_node is goal:
print('Success. Route from {} to {} found. Cost: {} Path: {}'.format(start,goal,current_cost,list(reconstructPath(current_node))))
break
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors:
if each_neighbor in closedSet: # Better parent sub algorithm?!!
tentative_gScore = totalDistance(each_neighbor) # neighbor already in closed set, so there is previous parent for sure
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
if (gScore < tentative_gScore): # current_node is better parent for this neighbor
cameFrom[each_neighbor] = current_node
if each_neighbor not in closedSet:
#gScore = 'cost till current_node' + 'cost between current node and its neighbor'
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
#print(openSet.queue) #just to verify
return 'No Goal found!'
M = romania_location_map
start = 'A'
goal = 'U'
result = AStar(start, goal)
Holy Moly Shit!! It worked!! Whew!!!
Minor optimzations for those who can't resist
Obviously main loop can be further optimized that instead of 2 'if' statements, we could have one. I prefer this unoptimized state, because it reminds us the exact context we built the algorithm from the beginning. However, some cannot stand that, so let us see optimized form also.
Current form:
for each_neighbor in all_neighbors:
if each_neighbor in closedSet: # Better parent sub algorithm?!!
tentative_gScore = totalDistance(each_neighbor)
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
if (gScore < tentative_gScore): # current_node is better parent for this neighbor
cameFrom[each_neighbor] = current_node
if each_neighbor not in closedSet:
#gScore = 'cost till current_node' + 'cost between current node and its neighbor'
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
Note gScore is same, so can be taken out. Also 2nd if is just else.
for each_neighbor in all_neighbors:
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
if each_neighbor in closedSet: # Better parent sub algorithm?!!
tentative_gScore = totalDistance(each_neighbor)
if (gScore < tentative_gScore): # current_node is better parent for this neighbor
cameFrom[each_neighbor] = current_node
else:
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
cameFrom[each_neighbor] = current_node
closedSet.add(each_neighbor)
cameFrom[each_neighbor] = current_node is also done in both if and else. So in the gScore comparision, we could simply skip to next iteration of for loop, on opposite condition, ensure otherwise irrespective of neighbor is already visited or not. That is, if gScore >= tentative_gScore we just continue, with moving cameFrom update outside if else.
for each_neighbor in all_neighbors:
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
if each_neighbor in closedSet:
tentative_gScore = totalDistance(each_neighbor)
if (gScore >= tentative_gScore):
continue # we just skip to next iteration of for loop if current parent is good enough
else:
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
closedSet.add(each_neighbor)
cameFrom[each_neighbor] = current_node # if we didn't skip, this has to happen anways visited or not
I hope this is good enough. Let us hope this works.
from queue import PriorityQueue
from docHelpers_ipython import romania_location_map
from math import floor
import geopy
cameFrom = { }
# NEEDED FUNCTION TO CALCULATE DISTANCE FROM CURRENT NODE BACK TO START
def totalDistance(current_node):
cost = 0
while current_node is not None:
parent = cameFrom.get(current_node, None)
if parent is not None:
cost += distance(current_node, parent)
current_node = parent
return cost
def distance(start_node, end_node):
(x0,y0) = M[start_node]['pos']
(x1,y1) = M[end_node]['pos']
return floor(geopy.distance.geodesic((y0,x0), (y1,x1)).miles)
def reconstructPath(current_node):
Path = [current_node]
while current_node is not None:
current_node = cameFrom[current_node]
Path.append(current_node)
return reversed(Path[:-1])
def AStar(start, goal):
# INITIALIZATION
openSet = PriorityQueue()
openSet.put((0,start))
closedSet = set(start)
cameFrom[start] = None
# MAIN LOOP
while not openSet.empty():
current_cost, current_node = openSet.get()
if current_node is goal:
print('Success. Route from {} to {} found. Cost: {} Path: {}'.format(start,goal,current_cost,list(reconstructPath(current_node))))
break
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors:
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
if each_neighbor in closedSet: # Better parent sub algorithm?!!
tentative_gScore = totalDistance(each_neighbor) # neighbor already in closed set, so there is previous parent for sure
if (gScore >= tentative_gScore): # current_node is better parent for this neighbor
continue
else:
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
closedSet.add(each_neighbor)
cameFrom[each_neighbor] = current_node # if we didn't 'continue', this has to happen anways visited or not
#print(openSet.queue) #just to verify
return 'No Goal found!'
M = romania_location_map
start = 'A'
goal = 'U'
result = AStar(start, goal)
It works :)
Visualization (Optional)¶
We will inject a small code to keep track of bag contents, nodes traversed for sake of visualization. And then render an animation to visualize how nodes were traversed to reach the goal.
from queue import PriorityQueue
from docHelpers_ipython import romania_location_map
from math import floor
import geopy
# VISUALIZATION PURPOSE
from docHelpers_ipython import Doc
from IPython.display import display, Image
cameFrom = { }
# NEEDED FUNCTION TO CALCULATE DISTANCE FROM CURRENT NODE BACK TO START
def totalDistance(current_node):
cost = 0
while current_node is not None:
parent = cameFrom.get(current_node, None)
if parent is not None:
cost += distance(current_node, parent)
current_node = parent
return cost
def distance(start_node, end_node):
(x0,y0) = M[start_node]['pos']
(x1,y1) = M[end_node]['pos']
return floor(geopy.distance.geodesic((y0,x0), (y1,x1)).miles)
def reconstructPath(current_node):
Path = [current_node]
while current_node is not None:
current_node = cameFrom[current_node]
Path.append(current_node)
return reversed(Path[:-1])
def AStar(start, goal):
# INITIALIZATION
openSet = PriorityQueue()
openSet.put((0,start))
closedSet = set(start)
cameFrom[start] = None
# MAIN LOOP
while not openSet.empty():
current_cost, current_node = openSet.get()
if current_node is goal:
print('Success. Route from {} to {} found. Cost: {} Path: {}'.format(start,goal,current_cost,list(reconstructPath(current_node))))
break
# VISUALIZATION PURPOSE
all_neighbors = reversed(M.get(current_node,[]).get('connections',[]))
considered_neighbors = list(set(all_neighbors) - set(closedSet)) # thank you: https://stackoverflow.com/questions/3462143/get-difference-between-two-lists
all_neighbors = M.get(current_node,[]).get('connections',[])
for each_neighbor in all_neighbors:
gScore = totalDistance(current_node) + distance(current_node,each_neighbor)
if each_neighbor in closedSet: # Better parent sub algorithm?!!
tentative_gScore = totalDistance(each_neighbor) # neighbor already in closed set, so there is previous parent for sure
if (gScore >= tentative_gScore): # current_node is better parent for this neighbor
continue
else:
hScore = distance(each_neighbor, goal)
fScore = gScore + hScore
openSet.put((fScore, each_neighbor))
closedSet.add(each_neighbor)
cameFrom[each_neighbor] = current_node # if we didn't 'continue', this has to happen anways visited or not
# VISUALIZATION PURPOSE
_ = doc.updateParent(each_neighbor, current_node)
# VISUALIZATION PURPOSE
_ = doc.computeGraphs(current_node, considered_neighbors)
#print(openSet.queue) #just to verify
# VISUALIZATION PURPOSE
_ = doc.computeGraphs(current_node, [])
return 'No Goal found!'
M = romania_location_map
start = 'A'
goal = 'U'
# VISUALIZATION PURPOSE - called here caz we use doc in ourSearchAlgo
doc = Doc(M)
result = AStar(start, goal)
# VISUALIZATION PURPOSE
images = doc.render()
display(Image(images[0]),Image(images[1]),Image(images[2]))
Are we done? Are we really really done?¶
I wish. But there is still one more trap we need to solve. Try finding the route for 'V' to 'C' ;)
Do not worry. It is not as long or complicated as what we just solved. Much simpler.
Hint: This is not algorithm problem per se.
